//Author: Austen Higgins-Cassidy
// 4/10/2012


//16 Bit word
//Implementing 1.7 of the dcpu-16 spec by Notch
//Hardware implemented in LUA
#include <stdlib.h>
#include <fstream>
#include <lua.hpp>
#include "./assembler.h"
#include "./dcpu.h"
#include "./hw_interface.h"

#define ASSEMBLER
//C functions!
#define DEBUG_LOG

#ifdef DEBUG_LOG
std::map<unsigned short, bool> FLAGGED;
#endif

#define DEBUG_STEP
/*
enum inst{
	reg_adr = 0x00,
	reg_val = 0x08,
	reg_nxt = 0x10,
	pop = 0x18,
	pek = 0x19,
	psh = 0x1a,
	sp_reg = 0x1b,
	pc_reg = 0x1c,
	o_reg  = 0x1d,
	nxt_val = 0x1e,
	nxt_lit = 0x1f,
	lit = 0x20
};
*/
//Format aaaaaabbbbbooooo

unsigned short RAM[RAM_SIZE]; //0x10000 words of RAM
unsigned short REG[8]; //8 registers
//unsigned short HW_ID[MAX_HW];
//char *HW_MODEL[MAX_HW];
unsigned short IQC; //INTERRUPT QUEUE COUNT
IQN *IQH; //INTERRUPT QUEUE HEAD
IQN *IQL; //INTERRUPT QUEUE LAST (TAIL)
bool QUEUEING; //Queing interupts?
unsigned short pc; //PROGRAM COUNTER
unsigned short sp; //REVERSE STACK (starts at 0xffff)
unsigned short ex; //Extra/Excess reg
unsigned short ia; //Interrupt addr
unsigned short tmp_reg; //Special temp register
//EMULATION INFORMATION
//We are not designing a 1-1 model, we must artificially keep track of cycles
unsigned long cycle_count; //CYCLE EMULATION
unsigned short prog_length; //PROGRAM LENGTH

extern unsigned short HWC;
extern hw_device *HW[MAX_HW];
extern lua_State *lua_hw;

enum op_codes{
	op_nbsc = 0x0000, //SEE NBSC_CODES
	op_set = 0x0001, //1 + a + b
	op_add, //2 + b + a
	op_sub, //2 + b + a
	op_mul, //2 + b + a
	op_mli, //2 + b + a
	op_div, //3 + b + a
	op_dvi, //3 + b + a
	op_mod, //3 + b + a
	op_mdi, //3 + b + a
	op_and, //1 + b + a
	op_bor, //1 + b + a
	op_xor, //1 + b + a
	op_shr, //2 + b + a
	op_asr, //2 + b + a
	op_shl, //2 + b + a
	op_ifb, //2 + b + a + (?) 1
	op_ifc, //2 + b + a + (?) 1
	op_ife, //2 + b + a + (?) 1
	op_ifn, //2 + b + a + (?) 1
	op_ifg, //2 + b + a + (?) 1
	op_ifa, //2 + b + a + (?) 1
	op_ifl, //2 + b + a + (?) 1
	op_ifu, //2 + b + a + (?) 1
	op_adx = 0x001a, //3 + b + a
	op_sbx, //3 + b + a
	op_sti = 0x001e, //2 + b + a
	op_std, //2 + b + a
	op_jsr = 0x0020, //3 + a
	op_int = 0x0027, //4 + a
	op_iag, //1 + a
	op_ias, //1 + a
	op_rfi, //3 + a
	op_iaq, //2 + a
	op_hwn = 0x002f, //2 + a
	op_hwq, //4 + a
	op_hwi //4 + a + ?

};


void _reset(){
	for(unsigned int i = 0; i < RAM_SIZE; ++i){
		RAM[i] = 0;
	}
	//for(unsigned int i = 0; i < INTERRUPT_SIZE; ++i){
	//	INTERRUPT[i] = 0;
	//}
	pc = 0;
	sp = RAM_SIZE-1;//sp = 0xffff; //Reverse stack
	ex = 0;
	tmp_reg = 0;
	QUEUEING = false;
	//IQ = false;
	IQC = 0;
	IQH = NULL;
	IQL = IQH;
	//IQ = 0;
	//HWC = 0;
	//for(unsigned int i = 0; i < MAX_HW; ++i)
	//	HW[i] = NULL;
};

unsigned short* _readbin(std::ifstream &f, unsigned short &size){
	//FILE *f;
	unsigned short *inst;
	unsigned short buf;
	std::vector<unsigned short> vbuf;
	while(!f.eof()){
		f.read((char*)&buf, sizeof(unsigned short));
		vbuf.push_back(buf);
	}
	size = vbuf.size();
	inst = new unsigned short[size];

	for(int i = 0; i < size; ++i){
		inst[i] = vbuf[i];
	}
	return inst;
}

void _load(unsigned short *&prog, short size){
	//Takes an array of shorts
	//Loads them into ram
	//Sets SP to just past the program
	int i;
	for(i = 0; i <= size; ++i){
		RAM[i] = prog[i];
	}
	prog_length = size;
};

unsigned short* _lookup(unsigned short code, bool a = false){
	unsigned short *reg = NULL;
	//Is this a literal or reg lookup?
#ifdef DEBUG_LOG
	std::cout << "Looking up " << code << std::endl;
#endif
	if(code < 0x08){
#ifdef DEBUG_LOG
	std::cout << "Register value " << std::endl;
#endif 
		return &REG[code];
	}else if(code < 0x010){
#ifdef  DEBUG_LOG
		std::cout << "Ram Address" << std::endl;
		FLAGGED[REG[code-0x08]] = true;
#endif
		return &RAM[REG[code-0x08]];
		++cycle_count;
	}else if(code < 0x18){
#ifdef DEBUG_LOG
		std::cout << "Offset Ram Address" << std::endl;
		FLAGGED[RAM[pc]+REG[code-0x10]] = true;
#endif
		//Raw addr + offset (Next word + register)
		reg = &RAM[RAM[(pc)] + REG[code-0x10]];
		++pc; //PC already points at next word
	}else if(code == 0x18){
		//POP rstack
		if(a){
#ifdef DEBUG_LOG
		std::cout << "POP-ing stack" << std::endl;
		FLAGGED[RAM[sp]] = true;
#endif
		return &RAM[(sp++)];
		}else{
#ifdef DEBUG_LOG
		std::cout << "PUSH-ing stack" << std::endl;
		FLAGGED[RAM[sp]] = true;
#endif
		return &RAM[(sp--)];
		}
	}else if(code == 0x19){
		//PEEK rstack
#ifdef DEBUG_LOG
		std::cout << "PEEK-ing stack" << std::endl;
		FLAGGED[RAM[sp]] = true;
#endif
		return &RAM[(sp)];
	}else if(code == 0x1a){
#ifdef DEBUG_LOG
		std::cout << "PICK-ing" << std::endl;
		FLAGGED[RAM[sp]] = true;
#endif
		return &RAM[(sp)+RAM[pc]];
		++pc;
	}else if(code == 0x1b){
#ifdef DEBUG_LOG
		std::cout << "Stack pointer" << std::endl;
#endif
		return &sp;
	}else if(code == 0x1c){
#ifdef DEBUG_LOG
		std::cout << "Program counter" << std::endl;
#endif
		return &pc;
	}else if(code == 0x1d){
#ifdef DEBUG_LOG
		std::cout << "EX reg" << std::endl;
#endif
		return &ex;
	}else if(code == 0x1e){
		//Raw addr
#ifdef DEBUG_LOG
		std::cout << "Ram at next word" << std::endl;
		FLAGGED[RAM[pc]] = true;
#endif
		//we are already set to the next word
		reg = &RAM[RAM[(pc)]]; //Ram-space PC reference
		++pc; //Consume the word
	}else if(code == 0x1f){
#ifdef DEBUG_LOG
		std::cout << "Next word" << std::endl;
		FLAGGED[pc] = true;
#endif
		//Next PC as a literal
		//we are already set to the next word
		reg = &RAM[(pc)]; //Program-space PC reference
		++pc; //Consume the word
	}else {
#ifdef DEBUG_LOG
		std::cout << "Literal value" << std::endl;
#endif
		tmp_reg = ((short)(code-0x20)); //Decode
		return &tmp_reg; //Persistant temporary register
	}
	++cycle_count;
	return reg;
}

void _push(unsigned short *v){
	--sp;
	RAM[sp] = *v;
#ifdef DEBUG_LOG
		std::cout << "Pushing ";
		WriteHex(std::cout, *v );
		std::cout << " at ";
		WriteHex(std::cout, sp);
		std::cout << std::endl;
#endif
}

void _pop(unsigned short *v){
	*v = RAM[sp]; 
	++sp;
#ifdef DEBUG_LOG
		std::cout <<"Poping ";
		WriteHex(std::cout, sp);
		std::cout << std::endl;
#endif
}
void _peek(unsigned short *v){
	*v = RAM[sp];
#ifdef DEBUG_LOG
		std::cout << "Peeking at ";
		WriteHex(std::cout, sp);
		std::cout << std::endl;
#endif
}

void _progdump(std::ostream &out){
	out << "##################\nProgram:\n";
	for(int i = 0; i < prog_length; ++i){
		WriteHex(out, RAM[i]);
	}
	out << "##################\n";
}

void _memdump(std::ostream &out){
	out << "##################\nStack:\n";
	for(unsigned short i = sp; i >= RAM_SIZE; ++i){
		out << "LOC ";
		WriteHex(out, i);
		out << ": ";
		WriteHex(out, RAM[i]);
		if(i%6 == 0)
			out << std::endl;
	}
	out << "##################\nRegisters:\n";
	out << "REG:   A       B       C       X       Y       Z       I       J       O\n";
	out << "     ";WriteHex(out, REG[0]);WriteHex(out, REG[1]);
	WriteHex(out, REG[2]);WriteHex(out, REG[3]);
	WriteHex(out, REG[4]);WriteHex(out, REG[5]);
	WriteHex(out, REG[6]);WriteHex(out, REG[7]);
	WriteHex(out, ex);
	out << "\n##################\n";
#ifdef DEBUG_LOG
	out << "Flagged Mem:\n";
	for(std::map<unsigned short, bool>::iterator iter = FLAGGED.begin(); iter != FLAGGED.end(); ++iter){
		if(iter->second){
			out << "LOC ";
			WriteHex(out, iter->first);
			out << ": ";
			WriteHex(out, RAM[iter->first]);
			out << std::endl;
		}
	}
	out << "##################\n";
#endif
}
void _noexec(){
	//Consume the next command, but do not execute it
	unsigned short a = ((RAM[pc])&OP_A) >> 10;
	unsigned short b = ((RAM[pc])&OP_B) >> 5;
	unsigned short op = ((RAM[pc])&OP_OP);
	++pc;
	if((op >= op_ifb) &&( op <= op_ifu)){
		++cycle_count;
		_noexec();
	}
	
	int tc = cycle_count;
	_lookup(a, true);
	_lookup(b, false);
	cycle_count = tc;
}



void _cpu(){
	//read from the program counter into cache regs
#ifdef DEBUG_LOG
	std::cout << "CPU Cycle: " << cycle_count << std::endl;
	std::cout << "Op Code: ";
	WriteHex(std::cout, RAM[pc]);
#endif
	unsigned short a = ((RAM[pc])&OP_A) >> 10;
	unsigned short b = ((RAM[pc])&OP_B) >> 5;
	unsigned short op = ((RAM[pc])&OP_OP);
	
#ifdef DEBUG_LOG
	std::cout << "Op Decode: ";
	WriteHex(std::cout, op);
	std::cout << " A: ";
	WriteHex(std::cout, a);
	std::cout << " B: ";
	WriteHex(std::cout, b);
	std::cout << std::endl;
	std::cout << "Program counter: ";
	WriteHex(std::cout, pc);
#endif
	++pc; //Icrememnt to the next word

	//We can't simply lookup here, because we have no idea how nonbasic commands will interact.
	//We could do WHOLE CPU optimization for it
	unsigned short *ref = NULL;
	unsigned short *ref_2 = NULL;
	int c = 0;
	switch(op){
	case op_nbsc: //SEE NBSC_CODES
		//In this case, a holds our opcode
		//B holds our value
		switch(b){
		case op_jsr:
			#ifdef DEBUG_LOG
			std::cout << "JSR OP" << std::endl;
			#endif
			//Push PC to the stack
			_push(&pc);
			//Set PC to a
			pc = *_lookup(a, true);
			cycle_count += 2;
			break;
		case op_int:
			#ifdef DEBUG_LOG
			std::cout << "INT OP" << std::endl;
			#endif
			if(ia != 0)
				if(QUEUEING){
					IQN *nt = new IQN;
					nt->msg = a;
					nt->next = NULL;
					IQL->next = nt;
					IQL = nt;
					++IQC;
					if(IQC == INTERRUPT_SIZE){
						std::cout << "Interrupt Queue Exceeded, DCPU-16 has caught fire" << std::endl;
						exit(0);
					}
				}else{
					QUEUEING = true; //This prevents Interrupt chaining
					_push(&pc);
					_push(&REG[0]);
					pc = ia;
					REG[0] = a;
				}
				cycle_count += 4;
			break;
		case op_iag:
			#ifdef DEBUG_LOG
			std::cout << "IAG OP" << std::endl;
			#endif
			ref = _lookup(a, true);
			*ref = ia;
			++cycle_count;
			break;
		case op_ias:
			#ifdef DEBUG_LOG
			std::cout << "IAS OP" << std::endl;
			#endif
			ref = _lookup(a, true);
			ia = *ref;
			++cycle_count;
			break;
		case op_rfi:
			#ifdef DEBUG_LOG
			std::cout << "RFI OP" << std::endl;
			#endif
			QUEUEING = false;
			_pop(&REG[0]);
			_pop(&pc);
			cycle_count += 3;
			break;
		case op_iaq:
			#ifdef DEBUG_LOG
			std::cout << "IAQ OP" << std::endl;
			#endif
			ref = _lookup(a, true);
			if(*ref != 0)
				QUEUEING = true;
			else
				QUEUEING = false;
			cycle_count += 2;
			break;
		case op_hwn:
			#ifdef DEBUG_LOG
			std::cout << "HWN OP" << std::endl;
			#endif
			ref = _lookup(a, true);
			*ref = HWC;
			cycle_count += 2;
			break;
		case op_hwq:
			#ifdef DEBUG_LOG
			std::cout << "HWQ OP" << std::endl;
			#endif
			ref = _lookup(a, true);
			//Set the id
			if(HW[*ref]){
			REG[0] = HW[*ref]->id >> 16;
			REG[1] = HW[*ref]->id;
			REG[2] = HW[*ref]->ver;
			REG[3] = HW[*ref]->man >> 16;
			REG[4] = HW[*ref]->man;
			}
			cycle_count += 4;
			break;
		case op_hwi:
			#ifdef DEBUG_LOG
			std::cout << "HWI OP" << std::endl;
			#endif
			ref = _lookup(a, true);
			if(HW[*ref])
				sendInterrupt(HW[*ref]);
			break;
		};
		break;
	case op_set: //1 + a + b
		#ifdef DEBUG_LOG
		std::cout << "SET OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		*ref = *ref_2;
		//Naturally fails when attempting to assign a literal due to the tmp_reg
		++cycle_count;
		break;
	case op_add: //2 + a + b
		#ifdef DEBUG_LOG
		std::cout << "ADD OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		c = (short)*ref + (short)*ref_2;
		(c > 0xFFFF) ? ex = 0x0001 : ex = 0x0;
		*ref = c;
		cycle_count += 2;
		break;
	case op_sub: //2 + a + b
		#ifdef DEBUG_LOG
		std::cout << "SUB OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		c = (short)*ref - (short)*ref_2;
		(c < -32768) ? ex = 0xffff : 0x0;
		*ref = c;
		cycle_count += 2;
		break;
	case op_mul: //2 + a + b
		#ifdef DEBUG_LOG
		std::cout << "MUL OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		ex = (((*ref)*(*ref_2))>>16)&0xffff;
		*ref = (*ref) * (*ref_2);
		cycle_count += 2;
		break;
	case op_mli:
		#ifdef DEBUG_LOG
		std::cout << "MIL OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		ex = (((*ref)*(*ref_2))>>16)&0xffff;
		c = (short)(*ref) * (short)(*ref_2);
		*ref = c;
		cycle_count += 2;
		break;
	case op_div: //3 + a + b
		#ifdef DEBUG_LOG
		std::cout << "DIV OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		if(*ref_2 != 0){
		ex = (((*ref)<<16)/(*ref_2))&0xffff;
		*ref = (*ref) / (*ref_2);
		} else {*ref = 0; ex = 0;}
		cycle_count += 3;
		break;
	case op_dvi:
		#ifdef DEBUG_LOG
		std::cout << "DVI OP" << std::endl;
		#endif
		//always rounds twoards zero
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		if(*ref_2 != 0){
		ex = (((*ref)<<16)/(*ref_2))&0xffff;
		*ref = (double)(*ref) / (double)(*ref_2);
		} else {*ref = 0; ex = 0;}
		cycle_count += 3;
		break;
	case op_mod: //3 + a + b
		#ifdef DEBUG_LOG
		std::cout << "MOD OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		if(*ref_2 != 0){
		*ref = (*ref) % (*ref_2);
		} else {*ref = 0;}
		cycle_count += 3;
		break;
	case op_mdi: //3 + a + b
		#ifdef DEBUG_LOG
		std::cout << "MDI OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		if(*ref_2 != 0){
			*ref = (short)(*ref)%(short)(*ref_2);
		}else {*ref = 0;}
		cycle_count += 3;
		break;
	case op_and: //1 + a + b
		#ifdef DEBUG_LOG
		std::cout << "AND OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		*ref = (*ref)&(*ref_2);
		++cycle_count;
		break;
	case op_bor: //1 + a + b
		#ifdef DEBUG_LOG
		std::cout << "BOR OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		*ref = (*ref)|(*ref_2);
		++cycle_count;
		break;
	case op_xor: //1 + a + b
		#ifdef DEBUG_LOG
		std::cout << "XOR OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		*ref = (*ref)^(*ref_2);
		++cycle_count;
		break;
	case op_shr:
		#ifdef DEBUG_LOG
		std::cout << "SHR OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		ex = (((*ref)<<16)>>(*ref_2))&0xffff;
		(*ref) >>= (*ref_2);
		cycle_count += 2;
		break;
	case op_asr:
		#ifdef DEBUG_LOG
		std::cout << "ASR OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		ex = (((*ref)<<16)>>(*ref_2))&0xffff;
		*ref = ((short)(*ref)) >> (*ref_2);
		cycle_count += 2;
		break;
	case op_shl: //2 + a + b
		#ifdef DEBUG_LOG
		std::cout << "SHL OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		ex = (((*ref)<<(*ref_2))>>16)&0xffff;
		(*ref) <<= (*ref_2);
		cycle_count += 2;
		break;
	case op_ifb: //2 + a + b + (?) 1
		#ifdef DEBUG_LOG
		std::cout << "IFB OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		if((*ref & *ref_2) != 0){
			cycle_count += 2;
		}else{
			cycle_count += 3;
			_noexec();
		}
		break;
	case op_ifc:
		#ifdef DEBUG_LOG
		std::cout << "IFC OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b,true);
		if((*ref & *ref_2) == 0){
			cycle_count += 2;
		}else{
			cycle_count += 3;
			_noexec();
		}
		break;
	case op_ife: //2 + a + b + (?) 1
		#ifdef DEBUG_LOG
		std::cout << "IFE OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		if(*ref == *ref_2){
			cycle_count += 2;
		}else{
			cycle_count += 3;
			_noexec();
		}
		break;
	case op_ifn: //2 + a + b + (?) 1
		#ifdef DEBUG_LOG
		std::cout << "IFN OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		if(*ref != *ref_2){
			cycle_count += 2;
		}else{
			cycle_count += 3;
			_noexec();
		}
		break;
	case op_ifg: //2 + a + b + (?) 1
		#ifdef DEBUG_LOG
		std::cout << "IFG OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		if(*ref > *ref_2){
			cycle_count += 2;
		}else{
			cycle_count += 3;
			_noexec();
		}
		break;
	case op_ifa: //2 + a + b + (?) 1
		#ifdef DEBUG_LOG
		std::cout << "IFA OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		if((short)*ref > (short)*ref_2){
			cycle_count += 2;
		}else{
			cycle_count += 3;
			_noexec();
		}
		break;
	case op_ifl: //2 + a + b + (?) 1
		#ifdef DEBUG_LOG
		std::cout << "IFL OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		if(*ref < *ref_2){
			cycle_count += 2;
		}else{
			cycle_count += 3;
			_noexec();
		}
		break;
	case op_ifu: //2 + a + b + (?) 1
		#ifdef DEBUG_LOG
		std::cout << "IFU OP" << std::endl;
		#endif
		ref_2 = _lookup(a,true);
		ref = _lookup(b);
		if((short)*ref < (short)*ref_2){
			cycle_count += 2;
		}else{
			cycle_count += 3;
			_noexec();
		}
		break;
	case op_adx:
		#ifdef DEBUG_LOG
		std::cout << "ADX OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		c = *ref + *ref_2 + ex;
		(c > 0xFFFF) ? ex = 0x0001 : ex = 0x0;
		*ref = c;
		cycle_count += 3;
		break;
	case op_sbx:
		#ifdef DEBUG_LOG
		std::cout << "SBX OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		c = *ref-*ref_2 + ex;
		(c <= -32767) ? ex = 0xffff : ex = 0x0;
		*ref = c;
		cycle_count += 3;
		break;
	case op_sti:
		#ifdef DEBUG_LOG
		std::cout << "STI OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		*ref = *ref_2;
		++REG[6];
		++REG[7];
		break;
	case op_std:
		#ifdef DEBUG_LOG
		std::cout << "STD OP" << std::endl;
		#endif
		ref_2 = _lookup(a, true);
		ref = _lookup(b);
		*ref = *ref_2;
		--REG[6];
		--REG[7];
		break;
	default:
		//Error
		break;		
	};
#ifdef DEBUG_LOG
	std::cout << std::endl;
	_memdump(std::cout);
#endif
	//INTERRUPT COORDINATOR
	//If we are NOT queuing, handle next interrupt in the queue
	if(!QUEUEING && IQH)
	{
		QUEUEING = true;
		_push(&pc);
		_push(&REG[0]);
		pc = ia;
		REG[0] = IQH->msg;
		--IQC;
		IQN *t = IQH->next;
		delete IQH;
		IQH = t;
	}
	for(int i = 0; i < HWC; ++i)
		sendUpdate(HW[i]);
}

void _error(char *str){
	std::cout << str << std::endl;
}

void _run(){
#ifdef DEBUG_LOG
	std::cout << "Executing Program" << std::endl;
	std::ofstream dfile;
	dfile.open("pc_dump.txt");
#endif
	while(pc <= prog_length){
		dfile << "PC | 0x" << std::hex << pc << " | 0x" << RAM[pc] << std::endl;
		_cpu();
#ifdef DEBUG_LOG
		
#endif
		//Ensure our stack pointer is valid
		if(sp < prog_length)
			_error("Stack/Program corrpution");
		if(sp < 0)
			_error("Memory out of bounds");

	}
#ifdef DEBUG_STEP
	dfile.close();
	std::cin.get();
#endif
}

int main(int argc, char *argv[]){
	
	std::ifstream bin_in;
	std::ofstream asm_out;
	std::ifstream asm_in;
	std::ofstream log_file;
	std::ostream *log_out;
	bool out_step=false;
	bool run_step=false;
	bool asm_step=false;
	lua_hw = luaL_newstate();
	luaL_openlibs(lua_hw);
	openLibHW(lua_hw);
	_reset(); //Prepare the emulator
	log_out = &std::cout;
	for(int i = 1; i < argc; ++i){
		if(strcmp(argv[i], "-o") == 0){
			out_step = true;
			asm_out.open(argv[++i], std::ios::binary);
			if(!asm_out.is_open()){
				*log_out << "Could not open " << argv[i] << std::endl;
			}
		}else if(strcmp(argv[i], "-a") == 0){
			asm_step = true;
			asm_in.open(argv[++i]);
			if(!asm_in.is_open())
				*log_out << "Could not open " << argv[i] << std::endl;
		}else if(strcmp(argv[i], "-l") == 0){
			log_file.open(argv[++i]);
			if(!log_file.is_open())
				log_out = &log_file;
			else 
				*log_out << "Could not open " << argv[i] << std::endl;
		}else if(strcmp(argv[i], "-r") == 0){
			run_step = true;
			bin_in.open(argv[++i],std::ios::binary);
			if(!bin_in.is_open())
				*log_out << "Could not open " << argv[i] << std::endl;
		}else if(strcmp(argv[i], "-h") == 0){
			*log_out << "Loading HW" << std::endl;
			luaL_dofile(lua_hw, argv[++i]);
		}
	}
	unsigned short *tmp_bin=NULL;
	unsigned short size=0;
	if(asm_step&&run_step&&out_step){
		assemble(asm_in, asm_out, tmp_bin, size);
		_load(tmp_bin, size);
	}else if(asm_step&&run_step&&!out_step){
		assemble(asm_in, std::cout, tmp_bin, size);
		_load(tmp_bin, size);
	}else if(asm_step&&out_step&&!run_step){
		assemble(asm_in, asm_out);
		return 0;
	}else if(run_step&&!asm_step){
		tmp_bin = _readbin(bin_in, size);
		_load(tmp_bin, size);
	}
		_run();

	return 0;
}